#include "LidarProcesser.h"

// LidarProcesser::LidarProcesser()
// {
// }

LidarListener::LidarListener(QWidget *parent, std::string port, std::string angle_correction)
    : QWidget(parent), _port(port),
      cloud0(new pcl::PointCloud<pcl::PointXYZI>),
      cloud1(new pcl::PointCloud<pcl::PointXYZI>),
      cloud2(new pcl::PointCloud<pcl::PointXYZI>) 
{
    ReadAngleCorrectionFile(angle_correction, lstFramesBound, lstAzimuth_Offset, lstElevation);

    pcl_viewer = std::make_shared<PCLVisualizer>("RSPointCloudViewer");
    pcl_viewer->setBackgroundColor(0.0, 0.0, 0.0);
    pcl_viewer->addCoordinateSystem(1.0);
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl_viewer->addPointCloud<pcl::PointXYZI>(pcl_pointcloud, "lidar");
    pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "lidar");

    timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &LidarListener::updatePointCloud);
    timer->start(100);
}

void LidarListener::updatePointCloud()
{
    {
        const std::lock_guard<std::mutex> lock(mtx_viewer);
        pcl_viewer->spinOnce();
    }
}

void LidarListener::ProcessData(QByteArray datagram)
{
    std::vector<std::byte> bytes;
    bytes.reserve(datagram.size());
    for (char byte : datagram)
    {
        bytes.push_back(static_cast<std::byte>(byte));
    }
    lastFrameIndex = currentFrameIndex;
    if (bytes.size() < 1118)
        return;
    ReadPackageData(bytes, lstFramesBound, lstAzimuth_Offset, lstElevation);
    if (lastFrameIndex != currentFrameIndex)
    {
        std::cout << "Update Frame." << std::endl;
        try
        {
            if (currentFrameIndex == 0)
            {
                // onLidarReceive(_port, _position, _rotation, frameVertices2);
                {
                    pcl::io::savePCDFileASCII(_port + "_2.pcd", *cloud2);
                    std::cout << "Save File " << _port << "_2.pcd" << " Success.";

                    const std::lock_guard<std::mutex>
                        lock(mtx_viewer);
                    pcl_viewer->removePointCloud("lidar");
                    pcl_viewer->addPointCloud<pcl::PointXYZI>(cloud2, "lidar");
                }
            }
            else if (currentFrameIndex == 1)
            {
                // onLidarReceive(_port, _position, _rotation, frameVertices0);
                {
                    pcl::io::savePCDFileASCII(_port + "_0.pcd", *cloud0);
                    std::cout << "Save File " << _port << "_0.pcd" << " Success.";

                    const std::lock_guard<std::mutex> lock(mtx_viewer);
                    pcl_viewer->removePointCloud("lidar");
                    pcl_viewer->addPointCloud<pcl::PointXYZI>(cloud0, "lidar");
                }
            }
            else if (currentFrameIndex == 2)
            {
                // onLidarReceive(_port, _position, _rotation, frameVertices1);
                {
                    pcl::io::savePCDFileASCII(_port + "_1.pcd", *cloud1);
                    std::cout << "Save File " << _port << "_1.pcd" << " Success.";

                    const std::lock_guard<std::mutex> lock(mtx_viewer);
                    pcl_viewer->removePointCloud("lidar");
                    pcl_viewer->addPointCloud<pcl::PointXYZI>(cloud1, "lidar");
                }
            }
        }
        catch (const char *s)
        {
            qDebug() << s;
        }
    }
}

bool LidarListener::ReadPackageData(const std::vector<std::byte> bytes, const std::vector<std::tuple<double, double>> &lstFramesBound, std::vector<double> &lstAzimuth_Offset, std::vector<double> &lstElevation)
{
    std::array<std::byte, 6> bytesPackageHeader;
    std::array<std::byte, 6> bytesDataHeader;
    std::array<std::byte, 1034> bytesData;
    std::array<std::byte, 40> bytesDataFooter;
    std::array<std::byte, 32> bytesSafe;

    std::memcpy(&bytesPackageHeader, bytes.data(), 6);
    std::memcpy(&bytesDataHeader, bytes.data() + 6, 6);
    std::memcpy(&bytesData, bytes.data() + 6 + 6, 1034);
    std::memcpy(&bytesDataFooter, bytes.data() + 6 + 6 + 1034, 40);
    std::memcpy(&bytesSafe, bytes.data() + 6 + 6 + 1034 + 40, 32);

    std::byte ProtocolVersionMajor = bytesPackageHeader[2];
    std::byte ProtocolVersionMinor = bytesPackageHeader[3];
    if (!(static_cast<int>(ProtocolVersionMajor) == 4 && static_cast<int>(ProtocolVersionMinor) == 3))
    {
        return false;
    }

    std::byte ChannelNum = bytesDataHeader[0]; // 激光通道数128
    std::byte BlockNum = bytesDataHeader[1];
    std::byte DisUnit = bytesDataHeader[3];
    std::byte ReturnNum = bytesDataHeader[4];

    uint16_t Azimuth1 = ToInt16<bytesData.size()>(bytesData, 0);
    std::byte FineAzimuth1 = bytesData[2];

    std::array<std::byte, 512> DataBlock1;
    std::memcpy(&DataBlock1, bytesData.data() + 3, 512);
    double az1 = Azimuth1 * 0.01 + static_cast<int>(FineAzimuth1) * 0.01 / 256; // 码盘角度

    GetPoints(DataBlock1, static_cast<int>(DisUnit) * 0.001, az1, lstFramesBound, lstAzimuth_Offset, lstElevation);

    uint16_t Azimuth2 = ToInt16<bytesData.size()>(bytesData, 515);
    std::byte FineAzimuth2 = bytesData[515 + 2];

    std::array<std::byte, 512> DataBlock2;
    std::memcpy(&DataBlock2, bytesData.data() + 515 + 3, 512);
    double az2 = Azimuth2 * 0.01 + static_cast<int>(FineAzimuth2) * 0.01 / 256; // 码盘角度

    GetPoints(DataBlock2, static_cast<int>(DisUnit) * 0.001, az2, lstFramesBound, lstAzimuth_Offset, lstElevation);

    return true;
}

bool LidarListener::GetPoints(std::array<std::byte, 512> bytes, double DisUnit, double azimuth, std::vector<std::tuple<double, double>> lstFramesBound, std::vector<double> lstAzimuth_Offset, std::vector<double> lstElevation)
{
    int frameIndex = -1;
    for (int i = 0; i < 3; i++)
    {
        auto bound = lstFramesBound[i];
        if (azimuth >= std::get<0>(bound) && azimuth <= std::get<1>(bound))
        {
            frameIndex = i;
        }
        else if (std::get<0>(bound) > std::get<1>(bound))
        {
            if (azimuth >= std::get<0>(bound) || azimuth <= std::get<1>(bound))
            {
                frameIndex = i;
            }
        }
    }

    auto a = azimuth - std::get<0>(lstFramesBound[frameIndex]);
    if (a < 0)
        a += 360;
    auto baseHAngle = a * 2; // 码盘转1°   水平方位角变化2°
    // var hIndex = (int)((baseHAngle - 30) / 0.1);
    std::round((baseHAngle - 30) * 10);
    auto hIndex = (int)std::round((baseHAngle - 30) * 10);
    if (!(hIndex >= 0 && hIndex <= 1200 - 1))
    {
        return false;
    }

    // Console.WriteLine($"--{frameIndex} {hIndex}------------------------");

    auto x = bytes.size();

    for (int j = 0; j < bytes.size() / 4; j++)
    {
        int startIndex = j * 4;
        auto point = LidarPoint();

        point.Distance = ToInt16<bytes.size()>(bytes, startIndex);
        point.Reflectivity = bytes[startIndex + 2];

        auto hAngle = baseHAngle - lstAzimuth_Offset[j];
        auto vAngle = lstElevation[j];

        int index = 1200 * j + hIndex;

        point.z = static_cast<int16_t>((std::sin(vAngle * M_PI / 180.0) * point.Distance * DisUnit * 100));
        auto xy = std::cos(vAngle * M_PI / 180.0) * point.Distance * DisUnit;
        point.x = static_cast<int16_t>((xy * std::sin(hAngle * M_PI / 180.0) * 100));
        point.y = static_cast<int16_t>((xy * std::cos(hAngle * M_PI / 180.0) * 100));

        if (frameIndex == 0)
        {
            frameVertices0[index * 3] = point.x;
            frameVertices0[index * 3 + 1] = point.y;
            frameVertices0[index * 3 + 2] = point.z;
            frameData0[index] = point;
            cloud0->push_back(pcl::PointXYZI(point.x, point.y, point.z));
        }
        else if (frameIndex == 1)
        {
            frameVertices1[index * 3] = point.x;
            frameVertices1[index * 3 + 1] = point.y;
            frameVertices1[index * 3 + 2] = point.z;
            frameData1[index] = point;
            cloud1->push_back(pcl::PointXYZI(point.x, point.y, point.z));
        }

        else if (frameIndex == 2)
        {
            frameVertices2[index * 3] = point.x;
            frameVertices2[index * 3 + 1] = point.y;
            frameVertices2[index * 3 + 2] = point.z;
            frameData2[index] = point;
            cloud2->push_back(pcl::PointXYZI(point.x, point.y, point.z));
        }
    }
    currentFrameIndex = frameIndex;

    return true;
}

bool LidarListener::ReadAngleCorrectionFile(std::string path, std::vector<std::tuple<double, double>> &lstFramesBound, std::vector<double> &lstAzimuth_Offset, std::vector<double> &lstElevation)
{
    std::ifstream file(path, std::ios::binary);

    if (!file.is_open())
    {
        throw std::runtime_error("Failed to open file.");
        return false;
    }

    file.seekg(0, std::ios::end);
    std::streamsize fileSize = file.tellg();
    file.seekg(0, std::ios::beg);

    std::vector<std::byte> bytes(fileSize);
    file.read(reinterpret_cast<char *>(bytes.data()), fileSize);

    if (!file)
    {
        throw std::runtime_error("Failed to read file.");
        return false;
    }

    int ProtocolVersionMajor = static_cast<std::uint8_t>(bytes[2]);
    int ProtocolVersionMinor = static_cast<std::uint8_t>(bytes[3]);
    if (!(ProtocolVersionMajor == 1 && ProtocolVersionMinor == 5))
    {
        return false;
    }

    int Resolution = static_cast<std::uint8_t>(bytes[16 - 1]);

    std::array<std::byte, 4 * 3> bytesStart_Frame;
    std::array<std::byte, 4 * 3> bytesEnd_Frame;
    std::array<std::byte, 4 * 128> bytesAzimuth_Offset;
    std::array<std::byte, 4 * 128> bytesElevation;

    std::memcpy(&bytesStart_Frame, bytes.data() + 16, 4 * 3);
    std::memcpy(&bytesEnd_Frame, bytes.data() + 16 + 12, 4 * 3);
    std::memcpy(&bytesAzimuth_Offset, bytes.data() + 16 + 12 * 2, 4 * 128);
    std::memcpy(&bytesElevation, bytes.data() + 16 + 12 * 2 + 4 * 128, 4 * 128);

    auto frame1Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 0) * Resolution / (double)25600; // 22
    auto frame2Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 4) * Resolution / (double)25600; // 142
    auto frame3Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 8) * Resolution / (double)25600;

    auto frame1End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 0) * Resolution / (double)25600; // 142
    auto frame2End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 4) * Resolution / (double)25600; // 262
    auto frame3End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 8) * Resolution / (double)25600; // 22

    lstFramesBound = {
        std::make_tuple(frame1Start, frame1End),
        std::make_tuple(frame2Start, frame2End),
        std::make_tuple(frame3Start, frame3End)};

    for (int i = 1; i <= 128; i++)
    {
        auto offset = ToInt32<bytesAzimuth_Offset.size()>(bytesAzimuth_Offset, (i - 1) * 4) * Resolution / (double)25600;
        lstAzimuth_Offset.push_back(offset);
    }

    for (int i = 1; i <= 128; i++)
    {
        auto offset = ToInt32<bytesAzimuth_Offset.size()>(bytesElevation, (i - 1) * 4) * Resolution / (double)25600;
        lstElevation.push_back(offset);
    }

    return true;
}

template <size_t N>
uint32_t LidarListener::ToUInt32(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 4 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    uint32_t result = 0;
    for (size_t i = 0; i < 4; ++i)
    {
        result += static_cast<uint32_t>(bytes[startIndex + i]) << (i * 8);
    }
    return result;
}

template <size_t N>
int32_t LidarListener::ToInt32(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 4 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    int32_t result = 0;
    for (size_t i = 0; i < 4; ++i)
    {
        result += static_cast<std::int32_t>(bytes[startIndex + i]) << (i * 8);
    }
    return result;
}

template <size_t N>
int16_t LidarListener::ToInt16(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 2 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    int16_t result = 0;
    for (size_t i = 0; i < 2; ++i)
    {
        result += (static_cast<std::int32_t>(bytes[startIndex + i]) << (i * 8));
    }

    return result;
}
